#include <errno.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/cm3/systick.h>
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/stm32/i2c.h>
#include <libopencm3/stm32/dma.h>

extern "C" {
    void sys_tick_handler(void);
}

uint8_t tx_buf[34] = {0x55};
uint8_t rx_buf[34] = {0xAA};
volatile uint8_t *read_p;
volatile uint8_t *write_p;
volatile uint8_t writing;
volatile uint8_t reading;

volatile uint8_t buf[3];
volatile uint16_t val;

static void clock_setup() {
    // First, let's ensure that our clock is running off the high-speed internal
    // oscillator (HSI) at 48MHz.
    rcc_clock_setup_in_hsi_out_48mhz();
    rcc_set_hpre(RCC_CFGR_HPRE_DIV2);
    rcc_ahb_frequency = 24000000;

    // Since our LED is on GPIO bank A, we need to enable
    // the peripheral clock to this GPIO bank in order to use it.
    rcc_periph_clock_enable(RCC_GPIOA);
    rcc_periph_clock_enable(RCC_GPIOB);

    // In order to use our UART, we must enable the clock to it as well.
    rcc_periph_clock_enable(RCC_USART1);
}

static void systick_setup() {
    // Set the systick clock source to our main clock
    systick_set_clocksource(STK_CSR_CLKSOURCE_AHB);
    // Clear the Current Value Register so that we start at 0
    STK_CVR = 0;
    // In order to trigger an interrupt every millisecond, we can set the reload
    // value to be the speed of the processor / 1000 -1
    systick_set_reload(rcc_ahb_frequency / 1000 - 1);
    // Enable interrupts from the system tick clock
    systick_interrupt_enable();
    // Enable the system tick counter
    systick_counter_enable();
}

// Storage for our monotonic system clock.
// Note that it needs to be volatile since we're modifying it from an interrupt.
static volatile uint64_t _millis = 0;

uint64_t millis() {
    return _millis;
}

// This is our interrupt handler for the systick reload interrupt.
// The full list of interrupt services routines that can be implemented is
// listed in libopencm3/include/libopencm3/stm32/f0/nvic.h
void sys_tick_handler(void) {
    // Increment our monotonic clock
    _millis++;
}

/**
 * Delay for a real number of milliseconds
 */
void delay(uint64_t duration) {
    const uint64_t until = millis() + duration;
    while (millis() < until);
}

static void gpio_setup() {
    // Our test LED is connected to Port A pin 5, so let's set it as output
    gpio_mode_setup(GPIOA, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO5);
}

static void start_mco(void){
	/* PA8 to AF 0 for MCO */
	rcc_periph_clock_enable(RCC_GPIOA);
	gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8);
	gpio_set_output_options(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO8);
	gpio_set_af(GPIOA, 0, GPIO8);
	RCC_CFGR = (RCC_CFGR & ~RCC_CFGR_MCOPRE) | RCC_CFGR_MCOPRE_DIV64;
	rcc_set_mco(RCC_CFGR_MCO_SYSCLK);

  	rcc_periph_clock_enable(RCC_GPIOC);
	gpio_mode_setup(GPIOC, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO4 | GPIO5| GPIO6 | GPIO8);
	//gpio_set(GPIOC, GPIO6);
}

void init_i2c_dma()
{
	/*
	 * Using channel 2 for I2C1_TX
	 */
    /* Enable DMA1 clock */
    rcc_periph_clock_enable(RCC_DMA1);
    //nvic_enable_irq(NVIC_DMA1_CHANNEL2_3_IRQ);
	/* Reset DMA channel*/
	dma_channel_reset(DMA1, DMA_CHANNEL2);
	dma_set_peripheral_address(DMA1, DMA_CHANNEL2, (uint32_t)&I2C_TXDR(I2C1));
	dma_set_read_from_memory(DMA1, DMA_CHANNEL2);
    dma_disable_peripheral_increment_mode(DMA1, DMA_CHANNEL2);
	dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL2);
	dma_set_peripheral_size(DMA1, DMA_CHANNEL2, DMA_CCR_PSIZE_8BIT);
	dma_set_memory_size(DMA1, DMA_CHANNEL2, DMA_CCR_MSIZE_8BIT);
	dma_set_priority(DMA1, DMA_CHANNEL2, DMA_CCR_PL_VERY_HIGH);

	dma_channel_reset(DMA1, DMA_CHANNEL3);
	dma_set_peripheral_address(DMA1, DMA_CHANNEL3, (uint32_t)&I2C_RXDR(I2C1));
	dma_set_read_from_peripheral(DMA1, DMA_CHANNEL3);
    dma_disable_peripheral_increment_mode(DMA1, DMA_CHANNEL3);
	dma_enable_memory_increment_mode(DMA1, DMA_CHANNEL3);
	dma_set_peripheral_size(DMA1, DMA_CHANNEL3, DMA_CCR_PSIZE_8BIT);
	dma_set_memory_size(DMA1, DMA_CHANNEL3, DMA_CCR_MSIZE_8BIT);
	dma_set_priority(DMA1, DMA_CHANNEL3, DMA_CCR_PL_VERY_HIGH);
}

void i2c_dma_send(uint8_t * p_tx, uint8_t tx_size)
{
	dma_set_memory_address(DMA1, DMA_CHANNEL2, (uint32_t)p_tx);
	dma_set_number_of_data(DMA1, DMA_CHANNEL2, tx_size);
	dma_enable_channel(DMA1, DMA_CHANNEL2);
    i2c_enable_txdma(I2C1);
}

void i2c_dma_recv(uint8_t * p_rx, uint8_t rx_size)
{
	dma_set_memory_address(DMA1, DMA_CHANNEL3, (uint32_t)p_rx);
	dma_set_number_of_data(DMA1, DMA_CHANNEL3, rx_size);
	dma_enable_channel(DMA1, DMA_CHANNEL3);
    i2c_enable_rxdma(I2C1);
}

void init_i2c(){
    uint8_t ownaddress = 0x40;

    for(int i=0; i<32; i++)
    {
        rx_buf[i] = i;
        tx_buf[i] = 32-i;
    }
	/* do i2c setup */
	rcc_periph_clock_enable(RCC_I2C1);
	rcc_periph_clock_enable(RCC_GPIOB);
	rcc_set_i2c_clock_hsi(I2C1);
	nvic_enable_irq(NVIC_I2C1_IRQ);

	i2c_reset(I2C1);
	gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO8 | GPIO9);
	gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, GPIO8 | GPIO9);
	gpio_set_af(GPIOB, GPIO_AF1, GPIO8 | GPIO9);
	i2c_peripheral_disable(I2C1);
	i2c_enable_analog_filter(I2C1);
	i2c_set_digital_filter(I2C1, 0);

	/* HSI is at 8Mhz */
	i2c_set_speed(I2C1, i2c_speed_fm_400k, 8);
	i2c_set_own_7bit_slave_address(I2C1, ownaddress);
    I2C_OAR1(I2C1) |= I2C_OAR1_OA1EN_ENABLE;
	//configure No-Stretch CR1 (only relevant in slave mode)
	i2c_enable_stretching(I2C1);
	I2C1_TIMINGR = 0x0010020B;
	//addressing mode
	i2c_set_7bit_addr_mode(I2C1);
	I2C_CR2(I2C1) &= ~I2C_CR2_NACK;
	i2c_peripheral_enable(I2C1);
    i2c_enable_interrupt(I2C1, I2C_CR1_ADDRIE | I2C_CR1_STOPIE);
    init_i2c_dma();
}

int main() {
    clock_setup();
    systick_setup();
    gpio_setup();
    start_mco();
    init_i2c();

    // Toggle the LED on and off forever
    while (1) {
        gpio_set(GPIOA, GPIO5);
        delay(1000);
        gpio_clear(GPIOA, GPIO5);
        delay(1000);
    }

    return 0;
}

void
i2c1_isr(void)
{
    //static uint32_t
	uint32_t sr1, sr2;
	sr1 = I2C_ISR(I2C1);
	// Address matched (Slave)
	if (sr1 & I2C_ISR_ADDR)
	{
	    gpio_set(GPIOC, GPIO6);
        i2c_dma_send(tx_buf, 34);
        i2c_dma_recv(rx_buf, 34);
		//Clear the ADDR FLAG.
        I2C_ICR(I2C1) |= I2C_ICR_ADDRCF;
        gpio_clear(GPIOC, GPIO6);
	}
    if(sr1 & I2C_ISR_STOPF)
    {
  	    gpio_set(GPIOC, GPIO8);
        i2c_disable_txdma(I2C1);
        dma_disable_channel(DMA1, DMA_CHANNEL2);
        I2C_ISR(I2C1) |= I2C_ISR_TXE;

        i2c_disable_rxdma(I2C1);
        dma_disable_channel(DMA1, DMA_CHANNEL3);

        i2c_clear_stop(I2C1);
  	    gpio_clear(GPIOC, GPIO8);
    }
}

void dma1_channel2_3_isr(void)
{
  	gpio_set(GPIOC, GPIO5);

	if ((DMA1_ISR &DMA_ISR_TCIF2) != 0) {
		DMA1_IFCR |= DMA_IFCR_CTCIF2;

	}
  	gpio_clear(GPIOC, GPIO5);
}
